package com.googlecode.curvedmetromaps.core.logic.forcedriven;

import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;
import org.jbox2d.dynamics.BodyDef;
import org.jbox2d.dynamics.BodyType;
import org.jbox2d.dynamics.World;

import playn.core.PlayN;

import com.googlecode.curvedmetromaps.core.model.MetroEdge;
import com.googlecode.curvedmetromaps.core.model.MetroLine;
import com.googlecode.curvedmetromaps.core.model.MetroMap;
import com.googlecode.curvedmetromaps.core.model.MetroStation;

import java.lang.management.ManagementFactory;
import java.lang.management.ThreadMXBean;
import java.util.ArrayList;

/**
 * This class implements a force-directed algorithm .
 */
public class ForceSystem {

	/** Main JBox2d object. */
	public World						world;

	public MetroMap						metroMap;

	/**
	 * Bodies in the JBox2D world that represent stations. The index is equal to
	 * the ID of the MetroStation, which is stored in the userData.
	 */
	public Body[]						stationBodies;

	/**
	 * Bodies in the JBox2D world that represent Bézier control points. The
	 * index is equal to the ID of the corresponding MetroEdge, which is stored
	 * in the userData.
	 */
	public Body[]						controlPointBodies;

	/**
	 * The scale factor between jbox and the model. model = modelScale * body
	 */
	public float						modelScale;

	/** The last update. */
	public double						lastUpdate			= PlayN.currentTime();

	/** The subsystems. */
	public ArrayList<ForceSubsystem>	subsystems			= new ArrayList<ForceSubsystem>();

	private double						simulationStart		= Double.POSITIVE_INFINITY;

	public double						cpuTime				= 0;

	private double						maxMaxForceFactor	= Double.NEGATIVE_INFINITY;
	public double						minMaxForceFactor	= Double.POSITIVE_INFINITY;
	public double						maxForceFactor	= Double.POSITIVE_INFINITY;
	
	public float speed = 0.00010f;

	/**
	 * Instantiates the JBox2D based force system.
	 * @param metroMap metro map which will be linked to the JBox2D world and
	 *            which will be modified after each call to update().
	 */
	public ForceSystem(MetroMap metroMap) {
		this.metroMap = metroMap;

		modelScale = calcModelScale(metroMap);

		// Create jbox2d world
		world = new World(new Vec2(0, 0));

		// Add bodies to world for stations
		createStationBodies(metroMap);
		// Add bodies for Bézier control points
		createControlPointBodies(metroMap);

		// Instantiate subsystems that apply forces for different constraints.
		subsystems.add(new ForceAdjacentStationAngles(this));
		subsystems.add(new ForceAspectRatio(this));
		subsystems.add(new ForceAttracktiveStations(this));
		subsystems.add(new ForceContinuity(this));
		subsystems.add(new ForceGeographicAnglePreservation(this));
		subsystems.add(new ForceRepulsiveStations(this));
		subsystems.add(new ForceRepulsiveEdges(this));

		// Disabled forces
		// subsystems.add(new ForceAngularResolution(this));
		// subsystems.add(new ForceCurveFitting(this));
		// subsystems.add(new ForceGeographicAttracktion(this));

		lastUpdate = PlayN.currentTime();

		// for (ForceSubsystem s : subsystems) {
		// System.out.print(s.getClass().getSimpleName() + "\t");
		// }
		// System.out.print("\n");
	}

	/**
	 * Performs a step in the JBox2D world and updates the model.
	 * @param delta Time in milliseconds that has passed since last update.
	 */
	public void update(int delta) {
		double startTime = PlayN.currentTime();

		if (Double.isInfinite(simulationStart)) {
			simulationStart = PlayN.currentTime();
		}

		double cpuStart = getCpuTime();
		double cpuFunctionTime = 0;
		
		while (cpuFunctionTime < 1) {

			// Calculate forces
			float totalForce = 0, totalX = 0, totalY = 0;
			float oldTotalForce = 0, oldX = 0, oldY = 0;

			for (ForceSubsystem s : subsystems) {
				s.applyForces(delta);
			}

			// Calculate average force
			Vec2 forceSum = new Vec2();
			for (int i = 0; i < stationBodies.length; i++) {
				Body body = stationBodies[i];
				forceSum.addLocal(body.m_force);
			}
			Vec2 forceAvg = forceSum.mulLocal(1f / stationBodies.length);
			
			// Make sure average force is zero.
			for (int i = 0; i < stationBodies.length; i++) {
				Body body = stationBodies[i];
				body.applyForceToCenter(forceAvg);
			}

			float maxForceSq = 0f;
			float xmin = Float.POSITIVE_INFINITY;
			float ymin = Float.POSITIVE_INFINITY;
			float xmax = Float.NEGATIVE_INFINITY;
			float ymax = Float.NEGATIVE_INFINITY;
			for (Body it = world.getBodyList(); it != null; it = it.getNext()) {
				Vec2 p = it.getPosition();
				xmin = Math.min(xmin, p.x);
				xmax = Math.max(xmax, p.x);
				ymin = Math.min(ymin, p.y);
				ymax = Math.max(ymax, p.y);

				float f = it.m_force.lengthSquared();
				if (maxForceSq < f)
					maxForceSq = f;
			}
			double bbSize = Math.sqrt((xmax - xmin) * (ymax - ymin));
			double maxForceFactor = Math.sqrt(maxForceSq) / bbSize;
			maxMaxForceFactor = Math.max(maxMaxForceFactor, maxForceFactor);
			minMaxForceFactor = Math.min(minMaxForceFactor, maxForceFactor);
			this.maxForceFactor = maxForceFactor;

			// Time in seconds since simulation started.
			// double simTime = PlayN.currentTime() - simulationStart;
			float dt = (float) ((Math.min(speed, (10f * speed) / maxForceFactor)));
			// float dt = 0.0005f;
			
			for (Body it = world.getBodyList(); it != null; it = it.getNext()) {
				if (!Float.isNaN(it.m_force.x) && !Float.isNaN(it.m_force.y)) {
					it.m_xf.p.addLocal(it.m_force.mul(dt));
					it.m_force.setZero();
				}
			}

			// Also apply stations body forces to control point position
			for (int i = 0; i < metroMap.lines.length; i++) {
				MetroLine metroLine = metroMap.lines[i];
				for (int j = 1; j < metroLine.stations.length; j++) {
					int m1 = metroLine.stations[j - 1].id;
					int m2 = metroLine.stations[j].id;
					Body b1 = stationBodies[m1];
					Body b2 = stationBodies[m2];
					int cpId = metroMap.getEdge(m1, m2).id;
					Body cpBody = controlPointBodies[cpId];
					
					cpBody.m_xf.p.addLocal(b1.m_force.add(b2.m_force).mulLocal(0.5f * dt * 0.9f));
				}
			}

			// Apply forces, then clear them
			for (Body it = world.getBodyList(); it != null; it = it.getNext()) {
				if (!Float.isNaN(it.m_force.x) && !Float.isNaN(it.m_force.y)) {
					it.m_xf.p.addLocal(it.m_force.mul(dt));
					it.m_force.setZero();
				}
			}

			updateModel();

			// Time at end of iteration
			cpuFunctionTime = (getCpuTime() - cpuStart) / 1E9;
		}
		
		double cpuEnd = getCpuTime();
		this.cpuTime += (cpuEnd - cpuStart) / 1E9;
	}

	/** Get CPU time in nanoseconds. */
	public long getCpuTime() {
		ThreadMXBean bean = ManagementFactory.getThreadMXBean();
		return bean.isCurrentThreadCpuTimeSupported() ? bean.getCurrentThreadCpuTime() : 0L;
	}

	/**
	 * Adds bodies to the world for the Bezier control points.
	 * @param metroMap the metro map which contains the list of stations.
	 */
	private void createControlPointBodies(MetroMap metroMap) {
		controlPointBodies = new Body[metroMap.edgeArray.length];

		// Loop through all edges
		for (MetroEdge edge : metroMap.edgeArray) {
			// Create body definition
			BodyDef edgeDef = new BodyDef();
			edgeDef.active = true;
			edgeDef.position = new Vec2(edge.cpX / modelScale, edge.cpY / modelScale);
			edgeDef.userData = edge;
			edgeDef.type = BodyType.DYNAMIC;
			// edgeDef.linearDamping = 0.0f;

			// Add new body to world;
			Body b = world.createBody(edgeDef);
			controlPointBodies[edge.id] = b;
		}
	}

	/**
	 * Adds bodies to the world that represent metro stations.
	 * @param metroMap the metro map which contains the list of stations.
	 */
	private void createStationBodies(MetroMap metroMap) {
		stationBodies = new Body[metroMap.stations.length];

		for (MetroStation m : metroMap.stations) {
			// Create metro station definition
			BodyDef stationDef = new BodyDef();
			stationDef.active = true;
			stationDef.position = new Vec2(m.x / modelScale, m.y / modelScale);
			stationDef.userData = m;
			stationDef.type = BodyType.DYNAMIC;
			// stationDef.linearDamping = 0.5f;

			// Add new body to world;
			Body b = world.createBody(stationDef);
			stationBodies[m.id] = b;
		}
	}

	/**
	 * Derives a model scale from a metro map, which can be used to scale the
	 * model into units which are appropriate for JBox2D.
	 * @param metroMap the metro map
	 * @return the calculated model scale
	 */
	private float calcModelScale(MetroMap metroMap) {
		// Derive model scale from the bounding box.
		float xmin = Float.POSITIVE_INFINITY;
		float xmax = Float.NEGATIVE_INFINITY;
		float ymin = Float.POSITIVE_INFINITY;
		float ymax = Float.NEGATIVE_INFINITY;
		for (MetroStation m : metroMap.stations) {
			xmin = Math.min(xmin, m.geoX);
			xmax = Math.max(xmax, m.geoX);
			ymin = Math.min(ymin, m.geoY);
			ymax = Math.max(ymax, m.geoY);
		}
		float dmax = Math.max(xmax - xmin, ymax - ymin);
		return (dmax != 0) ? (dmax / 100) : 1f;
	}

	/**
	 * Sets the position of all model elements based on the position within the
	 * JBox2D world.
	 */
	private void updateModel() {
		for (Body body : stationBodies) {
			MetroStation m = (MetroStation) body.getUserData();
			if (m != null) {
				Vec2 pos = body.getPosition();
				m.x = pos.x * modelScale;
				m.y = pos.y * modelScale;
				m.bsX *= modelScale;
				m.bsY *= modelScale;
				for (Vec2 v : m.edgeProj) {
					v.mulLocal(modelScale);
				}
			}
		}

		for (Body body : controlPointBodies) {
			MetroEdge e = (MetroEdge) body.getUserData();
			if (e != null) {
				Vec2 pos = body.getPosition();
				e.cpX = pos.x * modelScale;
				e.cpY = pos.y * modelScale;
			}
		}
	}
}
